/* car_control.c */

#include "app_inc.h"

/* #define MOTOR_PWM_COUNTER_MAX 300U */
#define SPEED_ENC_LIMIT 100

volatile PID_Hanlder_T gPidStructSpeedLeft, gPidStructSpeedRight;
volatile int32_t gLeftPwmCtrl, gRightPwmCtrl;
volatile int32_t gLeftEncCnt, gRightEncCnt;

extern volatile int16_t BL_AnglePidPwm;

void MoveCtrlSetup(void)
{
    PID_Init((PID_Hanlder_T *)(&gPidStructSpeedLeft), 1.8f, 0, 0.03f, SPEED_ENC_LIMIT);
    PID_SetTargetValue((PID_Hanlder_T *)(&gPidStructSpeedLeft), 2);

    PID_Init((PID_Hanlder_T *)(&gPidStructSpeedRight), 1.8f, 0, 0.03f, SPEED_ENC_LIMIT);
    PID_SetTargetValue((PID_Hanlder_T *)(&gPidStructSpeedRight), 2);

    gLeftPwmCtrl  = 0;
    gRightPwmCtrl = 0;
}

void MoveCtrlLoop(void)
{
    /* int32_t left_enc_speed, right_enc_speed; */

    /* Fetch the speed from encoder. */

    gLeftEncCnt  = 10000 - Motor_GetQuadDecoderValue(MOTOR_ID_A);
    gRightEncCnt = Motor_GetQuadDecoderValue(MOTOR_ID_B);
    Motor_ClearQuadDecoderValue(MOTOR_ID_A | MOTOR_ID_B);

    printf("L: %8d R: %8d\r\n", gLeftEncCnt, gRightEncCnt);
    if (gLeftEncCnt >= 5000)
    {
        gLeftEncCnt -= 10000;
    }
    if (gRightEncCnt >= 5000)
    {
        gRightEncCnt -= 10000;
    }


    /* Process the speed control. */
    gLeftPwmCtrl  += PID_GetIncCalc((PID_Hanlder_T *)(&gPidStructSpeedLeft),  gLeftEncCnt);
    gRightPwmCtrl += PID_GetIncCalc((PID_Hanlder_T *)(&gPidStructSpeedRight), gRightEncCnt);

    /* Process the angle control. */
    //gLeftPwmCtrl  += (-BL_AnglePidPwm);
    //gRightPwmCtrl += (-BL_AnglePidPwm);


    printf("gLeftPwmCtrl: %d, gRightPwmCtrl: %d\r\n", gLeftPwmCtrl, gRightPwmCtrl);

    /* Update the PWM for speed. */
#if 0
    if (gLeftPwmCtrl >= 0)
    {
        Motor_SetTurnDirection(MOTOR_ID_A, eMotor_DirectionForward);
			  if (gLeftPwmCtrl > MOTOR_PWM_COUNTER_MAX)
				{
						gLeftPwmCtrl = MOTOR_PWM_COUNTER_MAX
				}
        Motor_SetTrunSpeed(MOTOR_ID_A, gLeftPwmCtrl);
    }
    else /* gLeftPwmCtrl < 0 */
    {
        Motor_SetTurnDirection(MOTOR_ID_A, eMotor_DirectionBackward);
			  if (-gLeftPwmCtrl > MOTOR_PWM_COUNTER_MAX)
				{
				    gLeftPwmCtrl = -(int)(MOTOR_PWM_COUNTER_MAX);
				}
        Motor_SetTrunSpeed(MOTOR_ID_A, -gLeftPwmCtrl);
    }
#endif
#if 1
    if (gRightPwmCtrl >= 0)
    {
        Motor_SetTurnDirection(MOTOR_ID_B, eMotor_DirectionForward);
        Motor_SetTrunSpeed(MOTOR_ID_B, gRightPwmCtrl);
    }
    else
    {
        Motor_SetTurnDirection(MOTOR_ID_B, eMotor_DirectionBackward);
        Motor_SetTrunSpeed(MOTOR_ID_B, -gRightPwmCtrl);
    }
#endif
}

/* EOF. */

